﻿#include <cstdio>

#include <fstream>
#include <algorithm>
#include <camsense_lidar_driver/lidar.h>
#include <sstream>
//#include <glog/logging.h>
#include "../src/lidar_scan.h"
#pragma clang diagnostic push
#pragma ide diagnostic ignored "EndlessLoop"
int main(int argc, char **argv) {
//  google::InitGoogleLogging(argv[0]);
  std::string buff = "/dev/ttyS4";
  LidarScan lidar_device;
  std::vector<RangeData> data;

  std::vector<RangeData> show;
  std::mutex mtx;
  float last_angle = 0;
  lidar_device.Initialize([&](const std::vector<RangeData> &points) {
    for (const auto &point : points) {
      if (point.ori_theta > last_angle) {
        data.emplace_back(point);
      } else {
        std::cout<<point.ori_theta <<" " << last_angle << std::endl;
        {
          std::lock_guard<std::mutex> lk(mtx);
          show = data;
        }
        data.clear();
      }
      last_angle = point.ori_theta;
    }
  }, buff);
  getchar();
  lidar_device.Release();
  return 0;
}
#pragma clang diagnostic pop
